#pragma once
#include <algorithm>
#include <chrono>
#include <cmath>
#include <ctime>
#include <opencv2/core/mat.hpp>
#include <opencv2/opencv.hpp>

#include "KF.h"
#include "KFC.h"
#include "KalmanFilter.h"

/**
 * @brief 传入yaw和pitch角 对装甲板坐标系 即中心坐标转换
 * @param[in] 识别到的装甲板轨迹                    MyParam doc
 * @param[out]   输出滤波后的三维坐标并且可视化       MyParam doc
 * @author fx_03 (18249293810@qq.com)
 */
// 运动模型为三维匀速直线运动，对装甲板在车身坐标系下的中心点坐标进行滤波，将滤波结果坐标反投影回像素坐标系并显示，参照上周几个检测类的格式编写。
namespace hitcrt {
class KalmanFilter {
   public:
    KalmanFilter(const cv::Point3f unFilterPoint) {
        m_filter.emplace_back(hitcrt::KFC(2, 2));
        m_filter.emplace_back(hitcrt::KFC(2, 2));
        m_filter.emplace_back(hitcrt::KFC(2, 2));
        // 初始化只有创建的时候才有
        m_unFilterPoint = unFilterPoint;
        std::cout << "滤波器数量(初始化时输出): " << m_filter.size()
                  << std::endl;
    };
    void init(const std::vector<boost::any> &param);
    void updated(cv::Point3f lastPoint, cv::Point3f newPoint,
                 const std::vector<boost::any> &param);
    void getResult(cv::Point3f &resultKfilter);
        // 创建三个滤波器分别为XYZ位置

        std::vector<hitcrt::KFC> m_filter;
    cv::Point3f m_unFilterPoint;
    cv::Point3f m_FilterPoint;
    std::vector<double> m_processCov;
    std::vector<double> m_measureCov;
    double m_deltaTime;
};

}  // namespace hitcrt